#include <iostream>
#include <include.hpp>
#include <opencv2/opencv.hpp>
#include <cmath>
#include <fstream>
#include <filesystem>

#define b (0.55/2/M_PI)
double h_rk4 = 0.00001;//rk的步长
double h_direct_traversal = 0.0001;//二分法求零点的步长

template <typename T>
class circularQueue
{
public:
    T* arr = nullptr;
    int length = 0;
    int head = 0;
    int tail = 0;

    circularQueue(const int& length)
    {
        delete[] arr;
        arr = new T[length + 1];
        this->length = length;
        head = 0;
        tail = 0;
    }
    ~circularQueue(void)
    {
        delete[] arr;
    }
    circularQueue(void)
    {
        arr = nullptr;
    }

    bool init(const int& length)
    {
        delete[] arr;
        arr = new T[length + 1];
        this->length = length;
        head = 0;
        tail = 0;

        return 1;
    }

    bool clear(void)
    {
        head = 0;
        tail = 0;
        return 1;
    }

    bool is_full(void)
    {
        return head == (tail + 1) % (length + 1);
    }

    bool is_empty(void)
    {
        return head == tail;
    }

    int num_of_elem(void)
    {
        return (tail - head + length + 1) % (length + 1);
    }

    void push(const T& value)
    {
        if (is_full())//如果队列已满
        {
            head = (head + 1) % (length + 1);//抛弃队首值
        }
        arr[tail] = value;
        tail = (tail + 1) % (length + 1);
    }

    void pop(T& value)
    {
        if (is_empty())
        {
            std::cerr << "Error! Queue is empty!" << std::endl;
            return;
        }
        value = arr[head];
        head = (head + 1) % (length + 1);
    }

    T& operator[](const int& index)//自然是从队首（最旧值）开始，如index=0表示队首值
    {
        int index_tmp = index;
        while (1)
        {
            if (index_tmp > num_of_elem() - 1)
            {
                index_tmp = index_tmp - num_of_elem();
            }
            else if (index_tmp < 0)
            {
                index_tmp = index_tmp + num_of_elem();
            }
            else
            {
                break;
            }
        }
        return arr[(head + index_tmp) % (length + 1)];
    }
};

double* arr_theta_0_t;
double* arr_theta_0_and_dtheta_t;
double* arr_omega_0_t;

// RK4方法，y0为初始条件，t0为初始时间，h为步长，steps为步数
double rk4(double (*f)(const double&, const double&), const double& y0, const double& t0, const double& h, const int& steps)
{
    double t = t0, y = y0;
    for (int i = 0; i < steps; ++i) {
        double k1 = h * f(y, t);
        double k2 = h * f(y + 0.5 * k1, t + 0.5 * h);
        double k3 = h * f(y + 0.5 * k2, t + 0.5 * h);
        double k4 = h * f(y + k3, t + h);
        y += (k1 + 2 * k2 + 2 * k3 + k4) / 6;
        t += h;
    }
    return y;
}

//不动点迭代，返回自变量
double fixed_point_iteration_(double (*g)(const double&, const double&, const double&), const double& x_0, const double& k)
{
    double x = x_0;
    double x_last = __DBL_MIN__;
    double epsilon = 0.1;//收敛区域，越小越精确
    while (std::abs(x - x_last) > 0.1)
    {
        x_last = x;
        x = g(x, x_0, k);
        std::cout << "x: " << x << std::endl;
    }
    return x;
}

//直接遍历法
double direct_traversal_(double (*f)(const double&, const double&, const double&), const double& x_0, const double& k)
{
    double h = 0.1;//步长
    double x_l = x_0, x_r = x_l + h;
    while (f(x_l, x_0, k) * f(x_r, x_0, k) > 0)
    {
        x_l = x_r;
        x_r += h;
    }

    double mid = (x_l+x_r)/2;
    // double times = 10;//迭代次数
    double epsilon = h_direct_traversal;//收敛区域，越小越精确
    double f_l, f_r, f_mid;
    while (std::abs(x_l - x_r) > epsilon)
    {
        f_l = f(x_l, x_0, k);
        f_mid = f(mid, x_0, k);
        f_r = f(x_r, x_0, k);
        if (0 == f_mid)
        {
            break;
        }
        else if (f_l*f_mid < 0)
        {
            x_r = mid;
        }
        else
        {
            x_l = mid;
        }
        mid = (x_l+x_r)/2;
    }
    return mid;
}

double dtheta_divided_by_dt(const double& theta, const double& t)
{
    //返回dtheta/dt
    return -1/(b*sqrt(1+theta*theta));
}

void pre_calculate_theta_0_t(const int& start_time, const int& end_time)
{
    delete[] arr_theta_0_t;
    delete[] arr_theta_0_and_dtheta_t;
    arr_theta_0_t = new double[end_time+1];
    arr_theta_0_and_dtheta_t = new double[end_time+1];

    double theta_0 = 32 * M_PI, t_0 = start_time, t_end = end_time, h = h_rk4;//步长
    int steps = static_cast<int>((t_end - t_0) / h); // 计算步数

    double theta = theta_0;
    arr_theta_0_t[0] = theta;


    for (int i = 0; i < steps; ++i)
    {
        theta = rk4(dtheta_divided_by_dt, theta, t_0, h, 1);
        arr_theta_0_t[(int)(i*h)+1] = theta;


        if (0 == i)
        {
            arr_theta_0_and_dtheta_t[0] = theta;
        }
        else
        {
            arr_theta_0_and_dtheta_t[(int)((i-1)*h)+1] = theta;
        }
    }
}

//theta_0为初始条件，t_0为初始时间，h为步长，steps为步数
double theta_0_t(const double& t)
{
    // double theta_0 = 32 * M_PI, t_0 = 0, t_end = t, h = 0.000001;
    // int steps = static_cast<int>((t_end - t_0) / h); // 计算步数
    // double theta = theta_0;
    // for (int i = 0; i < steps; ++i)
    // {
    //     theta = rk4(dtheta_divided_by_dt, theta, t_0, h, 1);
    // }
    // return theta;
    return arr_theta_0_t[(int)t];
}



double f_theta_1(const double& theta_1, const double& theta_0, const double& k)
{
    return theta_0*theta_0 + theta_1*theta_1 - 2*theta_1*theta_0*cos(theta_0-theta_1)-k*k/(b*b);
}
double g_theta_1(const double& theta_1, const double& theta_0, const double& k)
{
    return theta_0*theta_0 + theta_1*theta_1 - 2*theta_1*theta_0*cos(theta_0-theta_1)-k*k/(b*b) + theta_1;
}

double theta_1_theta_0(const double& theta_0, const double& k)
{
    return direct_traversal_(f_theta_1, theta_0, k);
}

int main(void)
{


    double k_0 = 3.41-2*0.275;
    double k_1 = 2.2-2*0.275;
    pre_calculate_theta_0_t(0, 301);


    std::ofstream result1("/home/zwh/myStudies/C++/model_1/result1.csv");
    std::ofstream resultv("/home/zwh/myStudies/C++/model_1/v.csv");


    for (int i = 0; i < 301; ++i)
    {
        std::cout << arr_theta_0_t[i] << ", " << arr_theta_0_and_dtheta_t[i] << std::endl;
    }

    int t = 0;//时间
    double theta,theta_and_dtheta, omega, x, y, v;
    for (; t <= 300; ++t)
    {
        theta = theta_0_t(t);//第0个点
        x = b*theta*cos(theta);
        y = b*theta*sin(theta);
        theta_and_dtheta = arr_theta_0_and_dtheta_t[t];
        omega = (theta_and_dtheta-theta) / h_rk4;
        v = b*sqrt(1+theta*theta)*omega;
        result1 << x << ',' << y;
        resultv << -v;


        theta = theta_1_theta_0(theta, k_0);//第1个点
        x = b*theta*cos(theta);
        y = b*theta*sin(theta);
        theta_and_dtheta = theta_1_theta_0(theta_and_dtheta, k_0);
        omega = (theta_and_dtheta-theta) / h_rk4;
        v = b*sqrt(1+theta*theta)*omega;
        result1 << ',' << x << ',' << y;
        resultv << ',' << -v;
        // std::cout << "第" << 1 << "个点：" << theta << std::endl;

        for (int i = 2; i <= 223; ++i)
        {
            theta = theta_1_theta_0(theta, k_1);//之后的点
            x = b*theta*cos(theta);
            y = b*theta*sin(theta);
            theta_and_dtheta = theta_1_theta_0(theta_and_dtheta, k_1);
            omega = (theta_and_dtheta-theta) / h_rk4;
            v = b*sqrt(1+theta*theta)*omega;
            result1 << ',' << x << ',' << y;
            resultv << ',' << -v;
            // std::cout << "第" << i << "个点：" << theta << std::endl;
        }

        result1 << '\n';
        resultv << '\n';
        std::cout << "第" << t << "秒已生成！" << std::endl;
    }

    result1.close();

    return 0;
}

